#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

static void _calc_hist_lookup_table(const cv::Mat& hist, const float* range, std::vector<int>& table) {
    table.resize(256);
    int hist_size = hist.rows*hist.cols;
    
    double v_low = range[0];
    double v_high = range[1];
    double a = hist_size / (v_high - v_low);
    double b = -a * v_low;
    for (int i = 0; i < 256; i++) {
        int idx = cvFloor(i*a+b);
        int ii = -1;
        if (i >= v_low && i < v_high) {
            ii = (idx < 0 ? 0 : (idx > hist_size-1 ? hist_size-1 : idx));
        }
        table[i] = ii;
    }
}

static void _calc_back_project(const cv::Mat& src, const cv::Mat& hist, cv::Mat& backProject, const float* range, float scale) {
    std::vector<int> table;
    _calc_hist_lookup_table(hist, range, table);
    uchar pickupVals[256] = {0};
    for (int i = 0; i < 256; i++) {
        if (table[i] != -1) {
            pickupVals[i] = cv::saturate_cast<uchar>(hist.at<float>(table[i])*scale);
        }
    }
    backProject.create(src.size(), src.type());
    for (int y = 0; y < src.rows; y++) {
        for (int x = 0; x < src.cols; x++) {
            backProject.ptr<uchar>(y)[x] = pickupVals[src.ptr<uchar>(y)[x]];
        }
    }
}

int main(int argc, char **argv) {
    cv::Mat src = cv::imread("/home/xlll/Downloads/opencv/samples/data/home.jpg", cv::IMREAD_COLOR);
    if (src.empty()) {
        std::cout << "failed to read image" << std::endl;
        return EXIT_FAILURE;
    }
    
    cv::Mat hsv, hue;
    cv::cvtColor(src, hsv, cv::COLOR_BGR2HSV);
    cv::Mat hsv3[3];
    cv::split(hsv, hsv3);
    hue = hsv3[1];
    
    int hist_size = 20;
    float hue_range[] = {0, 180};
    const float* ranges = {hue_range};
    cv::Mat hist;
    cv::calcHist(&hue, 1, 0, cv::Mat(), hist, 1, &hist_size, &ranges, true, false);
    cv::normalize(hist, hist, 0, 255, cv::NORM_MINMAX, -1, cv::Mat());
    
    int width = 400, height = 400;
    int bin_width = cvRound((double)width / hist_size);
    cv::Mat hist_image = cv::Mat::zeros(width, height, CV_8UC1);
    for (int i = 0; i < hist_size; i++) {
        cv::rectangle(hist_image, cv::Point(i*bin_width, height), cv::Point((i+1)*bin_width, height-cvRound(hist.at<float>(i)*height / 255.0)), cv::Scalar(255), -1);
    }
    
    cv::Mat backproj1;
    cv::calcBackProject(&hue, 1, 0, hist, backproj1, &ranges, 1.2, true);
    cv::Mat backproj2;
    _calc_back_project(hue, hist, backproj2, hue_range, 1.2);
    
    cv::Mat matnonzeros = backproj1 != backproj2;
    std::vector<cv::Point> nonzeros;
    cv::findNonZero(matnonzeros, nonzeros);
    int size = nonzeros.size();
    std::cout << "nonzeros size = " << size << std::endl;
    
    cv::imshow("src", src);
    cv::imshow("hist", hist_image);
    cv::imshow("backproj", backproj1);
    cv::waitKey(0);
    
    
    return 0;
}
